from maix import camera , image ,  display , app , uart , time , nn
import struct

cam = camera.Camera(320,240)
dis = display.Display()
detector = nn.YOLOv5(model="/root/models/number1/model_120311.mud", dual_buff = True)
red_th = [[3, 86, 8, 74, -1, 53]] 
turn_flag = 0  # 转向标志位   0:不转向  1:左转   2:右转
cross_flag = 0 
run_task = 0   # 0 : 检测房间  1 ：开始运动
target_room = ""
count_time_l = 0
count_time_r = 0

rois = [
            [70,160,200,60],
            [70,80,200,60],
            [90,0,320,60]
        ]

roi_num =   [
                [0,150,150,90],
                [190,150,130,90]
            ]
device1 = "/dev/ttyS0"          # 示例的串口名称                        此串口设备名称为串口0
serial = uart.UART(device1,115200)  #创建串口对象，八位数据，无检验，一位停止


def Uart_Callback(uart,data):
    global run_task
    rx_len = len(data)
    if rx_len == 2:
        if data[0] == 0x55:
            run_task = data[1]
            print(run_task)


serial.set_received_callback(Uart_Callback)

def uart_send(serial:uart.UART,head,data:list):
    send_data = struct.pack("<Bhbb",head,data[0],data[1],data[2])
    serial.write(send_data)


def tracking(img:image.Image):
    global red_th , turn_flag
    cx = 0
    len = 0
    for i in range(2):
        blobs = img.find_blobs(red_th,roi=rois[i],area_threshold=200,pixels_threshold=200 )
        if blobs:
            blob = max(blobs , key = lambda blob:blob.area())
            img.draw_rect(blob.x(),blob.y(),blob.w(),blob.h(),color= image.COLOR_BLUE)
            cx += blob.cx()
            len += 1
    if len != 0:
        return (cx / len)
    else :
        return 5000

def judge_cross(img:image.Image):
    global cross_flag
    blobs = img.find_blobs(red_th,roi=[0,220,320,20],area_threshold=500,pixels_threshold=500 )
    if blobs:
        blob = max(blobs , key = lambda blob:blob.area())
        if blob.w() > 90 and blob.cy() > 210:
            cross_flag = 1
            img.draw_rect(blob.x(),blob.y(),blob.w(),blob.h(),color= image.COLOR_BLUE)
        else:
            cross_flag = 0
    else:
        cross_flag = 0

def judge_number(img:image.Image):
    global target_room , run_task , turn_flag , count_time_l , count_time_r
    objs = detector.detect(img, conf_th = 0.8, iou_th = 0.7)
    if run_task == 1:
        if target_room == "one":
            turn_flag = 1 
        elif target_room == "two":
            turn_flag = 2
        else:
            turn_flag = 0
            for obj in objs:
                img.draw_rect(obj.x, obj.y, obj.w, obj.h, color = image.COLOR_RED)
                msg = f'{detector.labels[obj.class_id]}: {obj.score:.2f}'
                img.draw_string(obj.x, obj.y, msg, color = image.COLOR_RED)
                if detector.labels[obj.class_id] == target_room:
                    cx = obj.x + (obj.w/2)
                    if cx > 190:
                        count_time_l += 1
                        if count_time_l > 5 :
                            turn_flag = 2
                            print("右转")
                    else:
                        count_time_r += 1
                        if count_time_r > 5 :
                            turn_flag = 2
                            print("右转")
                        turn_flag = 1
                        print("左转")
                else :
                    count_time_l = 0
                    count_time_r = 0
                    turn_flag = 0
                    print("直走")

            
    if run_task == 0:
        if objs:
            obj = max(objs,key=lambda obj:obj.score)
            target_room = detector.labels[obj.class_id]
            test = 0
            #run_task = 1


while not app.need_exit():
    img = cam.read()
    ex = 0
    judge_number(img)
    if run_task == 1:   
        tracking(img)
        judge_cross(img)
        judge_number(img)
        ex = tracking(img)
        if ex and ex != 5000:
            ex = ex - (img.width()/2)
            ex = int(ex)
        elif ex == 5000:
            ex = 5000
            print(1111)
        else :
            ex = 0

    '''串口通信协议:帧头0xaa 误差(int8_t) 十字路口标识(0:非,1:路口) 转弯标识(0:直走,1:左转,2:右转) '''
    uart_send(serial,0xaa,[ex,cross_flag,turn_flag])
    img.draw_string(0,80,f"turn is {turn_flag}")
    img.draw_string(0,0,f"err is {ex}")
    img.draw_string(0,40,f"cross is {cross_flag}")
    img.draw_string(250,0,f"task is {run_task}")
    img.draw_string(250,40,f"room is {target_room}")
    

    dis.show(img)